#!/home/he/anaconda3/envs/hellobot/bin/python
#  -*-coding:utf8 -*-

import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
startPos = [0,0,0.055]
startOrientation = p.getQuaternionFromEuler([0,0,0])     # Initial Orientation, play with it yourself
#robot = p.loadURDF("./Simulation/urdf/Simulation.urdf",startPos, startOrientation)
robot = p.loadURDF("/home/he/hellobot/src/hellobot_sim/src/PybulletSimulation/Simulation/urdf/Simulation.urdf",startPos, startOrientation)
#p.resetDebugVisualizerCamera(3.4,0,-55,[-2.5,1.5,0])        # Set default Camera Position, press ctrl+drag to change view
p.resetDebugVisualizerCamera(1,0,-55,[-5,3,0]) 
# p.changeVisualShape(robot,0,rgbaColor=[0.1,0.1,0.1,0.1])      # This Line Change the color and Transparency of body

#set the center of mass frame (loadURDF sets base link frame) startPos/Ornp.resetBasePositionAndOrientation(boxId, startPos, startOrientation)
v2 = 0
v1 = 0
dv = 0.1
for i in range (10000):
    keys = p.getKeyboardEvents()
    if ord('r') in keys and p.KEY_WAS_TRIGGERED:
        p.resetSimulation()
    if ord('u') in keys and p.KEY_IS_DOWN:
        v1 = v1 + dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=0,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v1)        # Velocity Control, Search this in Online Doc
    if ord('j') in keys and p.KEY_IS_DOWN:
        v1 = v1 - dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=0,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v1)
    if ord('i') in keys and p.KEY_IS_DOWN:                  # Watchout!!!! The positive direction is reversed
        v2 = v2 + dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=1,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v2)
    if ord('k') in keys and p.KEY_IS_DOWN:
        v2 = v2 - dv
        p.setJointMotorControl2(bodyIndex=robot,
                                jointIndex=1,
                                controlMode=p.VELOCITY_CONTROL,
                                targetVelocity=v2)
    p.stepSimulation()
    time.sleep(1./240.)
    if i%100==0:
        print(p.getBaseVelocity(bodyUniqueId=robot))    # This line Get the velocity of the base body
        print(p.getJointState(bodyUniqueId=robot,
                              jointIndex=0))        # This gets the joint state
        print(p.getJointState(bodyUniqueId=robot,
                              jointIndex=1))
        # getLinkState will give you the link states, like link velocity, orientation
p.disconnect()
